#include <dalelib/maths/m4.h>

namespace dalelib
{

const m4 m4::zero(0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0);
const m4 m4::identity(1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1);

m4::m4( const float* _Ptr )
{
	for(int i=0; i<16; ++i) f[i] = _Ptr[i];
}

m4::m4( const float _Value00, const float _Value01, const float _Value02, const float _Value03, const float _Value04, const float _Value05, const float _Value06, const float _Value07, const float _Value08, const float _Value09, const float _Value10, const float _Value11, const float _Value12, const float _Value13, const float _Value14, const float _Value15 )
{
	f[ 0] = _Value00; f[ 1] = _Value01; f[ 2] = _Value02; f[ 3] = _Value03;
	f[ 4] = _Value04; f[ 5] = _Value05; f[ 6] = _Value06; f[ 7] = _Value07;
	f[ 8] = _Value08; f[ 9] = _Value09; f[10] = _Value10; f[11] = _Value11;
	f[12] = _Value12; f[13] = _Value13; f[14] = _Value14; f[15] = _Value15;
}

m4::m4( const v4& _Diagonal )
{
	f[ 0] = _Diagonal[0]; f[ 1] = 0.f; f[ 2] = 0.f; f[ 3] = 0.f;
	f[ 4] = 0.f; f[ 5] = _Diagonal[1]; f[ 6] = 0.f; f[ 7] = 0.f;
	f[ 8] = 0.f; f[ 9] = 0.f; f[10] = _Diagonal[2]; f[11] = 0.f;
	f[12] = 0.f; f[13] = 0.f; f[14] = 0.f; f[15] = _Diagonal[3];
}

m4::m4( const v4& _Value0, const v4& _Value1, const v4& _Value2, const v4& _Value3 )
{
	f[ 0] = _Value0.x(); f[ 1] = _Value0.y(); f[ 2] = _Value0.z(); f[ 3] = _Value0.w();
	f[ 4] = _Value1.x(); f[ 5] = _Value1.y(); f[ 6] = _Value1.z(); f[ 7] = _Value1.w();
	f[ 8] = _Value2.x(); f[ 9] = _Value2.y(); f[10] = _Value2.z(); f[11] = _Value2.w();
	f[12] = _Value3.x(); f[13] = _Value3.y(); f[14] = _Value3.z(); f[15] = _Value3.w();
}

m4::m4( const m3& _Rot, const v3& _Pos )
{
	f[ 0] = _Rot[0]; f[ 1] = _Rot[1]; f[ 2] = _Rot[2]; f[ 3] = 0.f;
	f[ 4] = _Rot[3]; f[ 5] = _Rot[4]; f[ 6] = _Rot[5]; f[ 7] = 0.f;
	f[ 8] = _Rot[6]; f[ 9] = _Rot[7]; f[10] = _Rot[8]; f[11] = 0.f;
	f[12] = _Pos[0]; f[13] = _Pos[1]; f[14] = _Pos[2]; f[15] = 1.f;
}

m4::m4( const m4& _Mat )
{
	f[ 0] = _Mat[ 0]; f[ 1] = _Mat[ 1]; f[ 2] = _Mat[ 2]; f[ 3] = _Mat[ 3];
	f[ 4] = _Mat[ 4]; f[ 5] = _Mat[ 5]; f[ 6] = _Mat[ 6]; f[ 7] = _Mat[ 7];
	f[ 8] = _Mat[ 8]; f[ 9] = _Mat[ 9]; f[10] = _Mat[10]; f[11] = _Mat[11];
	f[12] = _Mat[12]; f[13] = _Mat[13]; f[14] = _Mat[14]; f[15] = _Mat[15];
}

//float& m4::operator[]( const int _Index ) { return f[_Index]; }

m4 m4::transpose() const
{
	return m4(
		f[ 0], f[ 4], f[ 8], f[12],
		f[ 1], f[ 5], f[ 9], f[13],
		f[ 2], f[ 6], f[10], f[14],
		f[ 3], f[ 7], f[11], f[15] );
}

m4 m4::fastInverse() const
{
	// fast inverse, must be orthonormal
	return m4(
		f[ 0], f[ 4], f[ 8], f[ 3],
		f[ 1], f[ 5], f[ 9], f[ 7],
		f[ 2], f[ 6], f[10], f[11],
		-f[ 0]*f[12] - f[ 1]*f[13] - f[ 2]*f[14],
		-f[ 4]*f[12] - f[ 5]*f[13] - f[ 6]*f[14],
		-f[ 8]*f[12] - f[ 9]*f[13] - f[10]*f[14],
		f[15] );

}

m4 m4::inverse() const
{
	m4 m;
	// Inverse = adjoint / det. (See linear algebra texts.)

    // pre-compute 2x2 dets for last two rows when computing
    // cofactors of first two rows.
    float d12 = (f[ 2]*f[ 7] - f[ 3]*f[ 6]);
    float d13 = (f[ 2]*f[11] - f[ 3]*f[10]);
    float d23 = (f[ 6]*f[11] - f[ 7]*f[10]);
    float d24 = (f[ 6]*f[15] - f[ 7]*f[14]);
    float d34 = (f[10]*f[15] - f[11]*f[14]);
    float d41 = (f[14]*f[ 3] - f[15]*f[ 2]);

    m[0] =  (f[ 5]*d34 - f[ 9]*d24 + f[13]*d23);
    m[1] = -(f[ 1]*d34 + f[ 9]*d41 + f[13]*d13);
    m[2] =  (f[ 1]*d24 + f[ 5]*d41 + f[13]*d12);
    m[3] = -(f[ 1]*d23 - f[ 5]*d13 + f[ 9]*d12);

    // Compute determinant as early as possible using these cofactors.
    float det = f[ 0]*m[0] + f[ 4]*m[1] + f[ 8]*m[2] + f[12]*m[3];

    // Run singularity test.
    if( det == 0.0 )
    {
//        cout << "Warning: Call to invertMatrix produced a Singular matrix." << endl;
		return m4::identity;
    }
    else
    {
       float invDet = 1.f/det;

       // Compute rest of inverse.
       m[0] *= invDet;
       m[1] *= invDet;
       m[2] *= invDet;
       m[3] *= invDet;

       m[4] = -(f[ 4]*d34 - f[ 8]*d24 + f[12]*d23) * invDet;
       m[5] =  (f[ 0]*d34 + f[ 8]*d41 + f[12]*d13) * invDet;
       m[6] = -(f[ 0]*d24 + f[ 4]*d41 + f[12]*d12) * invDet;
       m[7] =  (f[ 0]*d23 - f[ 4]*d13 + f[ 8]*d12) * invDet;

       // Pre-compute 2x2 dets for first two rows when computing cofactors
       // of last two rows.
       d12 = f[ 0]*f[ 5] - f[ 1]*f[ 4];
       d13 = f[ 0]*f[ 9] - f[ 1]*f[ 8];
       d23 = f[ 4]*f[ 9] - f[ 5]*f[ 8];
       d24 = f[ 4]*f[13] - f[ 5]*f[12];
       d34 = f[ 8]*f[13] - f[ 9]*f[12];
       d41 = f[12]*f[ 1] - f[13]*f[ 0];

       m[8]  =  (f[ 7]*d34 - f[11]*d24 + f[15]*d23) * invDet;
       m[9]  = -(f[ 3]*d34 + f[11]*d41 + f[15]*d13) * invDet;
       m[10] =  (f[ 3]*d24 + f[ 7]*d41 + f[15]*d12) * invDet;
       m[11] = -(f[ 3]*d23 - f[ 7]*d13 + f[11]*d12) * invDet;
       m[12] = -(f[ 6]*d34 - f[10]*d24 + f[14]*d23) * invDet;
       m[13] =  (f[ 2]*d34 + f[10]*d41 + f[14]*d13) * invDet;
       m[14] = -(f[ 2]*d24 + f[ 6]*d41 + f[14]*d12) * invDet;
       m[15] =  (f[ 2]*d23 - f[ 6]*d13 + f[10]*d12) * invDet;

    }

    return m;
}

m4 m4::noRotation() const
{
	return m4(
		1,0,0,f[ 3],
		0,1,0,f[ 7],
		0,0,1,f[11],
		f[12],f[13],f[14],f[15] );
}

m4 m4::noTranslation() const
{
	return m4(
		f[ 0],f[ 1],f[ 2],f[ 3],
		f[ 4],f[ 5],f[ 6],f[ 7],
		f[ 8],f[ 9],f[10],f[11],
		0,0,0,f[15] );
}

m4 m4::operator*( const float _Value ) const
{
	return m4(
		f[ 0]*_Value,f[ 1]*_Value,f[ 2]*_Value,f[ 3]*_Value,
		f[ 4]*_Value,f[ 5]*_Value,f[ 6]*_Value,f[ 7]*_Value,
		f[ 8]*_Value,f[ 9]*_Value,f[10]*_Value,f[11]*_Value,
		f[12]*_Value,f[13]*_Value,f[14]*_Value,f[15]*_Value );
}

v3 m4::transformVector( const v3& _Value ) const
{
	// pretend v3 is xyz0
	return v3(
		_Value[0]*f[0] + _Value[1]*f[4] + _Value[2]*f[ 8],
		_Value[0]*f[1] + _Value[1]*f[5] + _Value[2]*f[ 9],
		_Value[0]*f[2] + _Value[1]*f[6] + _Value[2]*f[10] );
}

v3 m4::transformPoint( const v3& _Value ) const
{
	// pretend v3 is xyz1
	return v3(
		_Value[0]*f[0] + _Value[1]*f[4] + _Value[2]*f[ 8] + f[12],
		_Value[0]*f[1] + _Value[1]*f[5] + _Value[2]*f[ 9] + f[13],
		_Value[0]*f[2] + _Value[1]*f[6] + _Value[2]*f[10] + f[14] );
}

v4 m4::operator*( const v4& _Value ) const
{
	return v4(
		v4::dot(v4(f[ 0],f[ 4],f[ 8],f[12]),_Value),
		v4::dot(v4(f[ 1],f[ 5],f[ 9],f[13]),_Value),
		v4::dot(v4(f[ 2],f[ 6],f[10],f[14]),_Value),
		v4::dot(v4(f[ 3],f[ 7],f[11],f[15]),_Value) );
}

m4 m4::operator*( const m4& _Value ) const
{
	const float* d = _Value.ptr();
	return m4(
		d[ 0]*f[0] + d[ 1]*f[4] + d[ 2]*f[ 8] + d[ 3]*f[12],
		d[ 0]*f[1] + d[ 1]*f[5] + d[ 2]*f[ 9] + d[ 3]*f[13],
		d[ 0]*f[2] + d[ 1]*f[6] + d[ 2]*f[10] + d[ 3]*f[14],
		d[ 0]*f[3] + d[ 1]*f[7] + d[ 2]*f[11] + d[ 3]*f[15],

		d[ 4]*f[0] + d[ 5]*f[4] + d[ 6]*f[ 8] + d[ 7]*f[12],
		d[ 4]*f[1] + d[ 5]*f[5] + d[ 6]*f[ 9] + d[ 7]*f[13],
		d[ 4]*f[2] + d[ 5]*f[6] + d[ 6]*f[10] + d[ 7]*f[14],
		d[ 4]*f[3] + d[ 5]*f[7] + d[ 6]*f[11] + d[ 7]*f[15],

		d[ 8]*f[0] + d[ 9]*f[4] + d[10]*f[ 8] + d[11]*f[12],
		d[ 8]*f[1] + d[ 9]*f[5] + d[10]*f[ 9] + d[11]*f[13],
		d[ 8]*f[2] + d[ 9]*f[6] + d[10]*f[10] + d[11]*f[14],
		d[ 8]*f[3] + d[ 9]*f[7] + d[10]*f[11] + d[11]*f[15],

		d[12]*f[0] + d[13]*f[4] + d[14]*f[ 8] + d[15]*f[12],
		d[12]*f[1] + d[13]*f[5] + d[14]*f[ 9] + d[15]*f[13],
		d[12]*f[2] + d[13]*f[6] + d[14]*f[10] + d[15]*f[14],
		d[12]*f[3] + d[13]*f[7] + d[14]*f[11] + d[15]*f[15]);
}

m4 m4::operator+( const m4& _Value ) const
{
	const float* d = _Value.ptr();
	return m4(
		f[ 0]+d[ 0], f[ 1]+d[ 1], f[ 2]+d[ 2], f[ 3]+d[ 3],
		f[ 4]+d[ 4], f[ 5]+d[ 5], f[ 6]+d[ 6], f[ 7]+d[ 7],
		f[ 8]+d[ 8], f[ 9]+d[ 9], f[10]+d[10], f[11]+d[11],
		f[12]+d[12], f[13]+d[13], f[14]+d[14], f[15]+d[15]);
}

m4 m4::operator-( const m4& _Value ) const
{
	const float* d = _Value.ptr();
	return m4(
		f[ 0]-d[ 0], f[ 1]-d[ 1], f[ 2]-d[ 2], f[ 3]-d[ 3],
		f[ 4]-d[ 4], f[ 5]-d[ 5], f[ 6]-d[ 6], f[ 7]-d[ 7],
		f[ 8]-d[ 8], f[ 9]-d[ 9], f[10]-d[10], f[11]-d[11],
		f[12]-d[12], f[13]-d[13], f[14]-d[14], f[15]-d[15]);
}


void m4::operator+=(const m4& _Value)
{
	f[ 0] += _Value[ 0]; f[ 1] += _Value[ 1]; f[ 2] += _Value[ 2]; f[ 3] += _Value[ 3];
	f[ 4] += _Value[ 4]; f[ 5] += _Value[ 5]; f[ 6] += _Value[ 6]; f[ 7] += _Value[ 7];
	f[ 8] += _Value[ 8]; f[ 9] += _Value[ 9]; f[10] += _Value[10]; f[11] += _Value[11];
	f[12] += _Value[12]; f[13] += _Value[13]; f[14] += _Value[14]; f[15] += _Value[15];
}

void m4::operator-=(const m4& _Value)
{
	f[ 0] -= _Value[ 0]; f[ 1] -= _Value[ 1]; f[ 2] -= _Value[ 2]; f[ 3] -= _Value[ 3];
	f[ 4] -= _Value[ 4]; f[ 5] -= _Value[ 5]; f[ 6] -= _Value[ 6]; f[ 7] -= _Value[ 7];
	f[ 8] -= _Value[ 8]; f[ 9] -= _Value[ 9]; f[10] -= _Value[10]; f[11] -= _Value[11];
	f[12] -= _Value[12]; f[13] -= _Value[13]; f[14] -= _Value[14]; f[15] -= _Value[15];
}

void m4::lookAt(const v3& _Pos,const v3& _Tgt,const v3& _Up)
{
#if 1
	v3 z = (_Tgt-_Pos).unit(), up = _Up;
	if(z.len()<0.00001f) { *this = m4::identity; return; }
	while(fabs(v3::dot(up,z))>0.999f) up = v3::randSphere();
	v3 x = v3::cross(z,up).unit();
	v3 y = v3::cross(x,z).unit();
	f[ 0]=x.x(); f[ 1]=x.y(); f[ 2]=x.z(); f[ 3]=0.f;
	f[ 4]=y.x(); f[ 5]=y.y(); f[ 6]=y.z(); f[ 7]=0.f;
	f[ 8]=z.x(); f[ 9]=z.y(); f[10]=z.z(); f[11]=0.f;
	f[12]=_Pos.x();  f[13]=_Pos.y();  f[14]=_Pos.z();  f[15]=1.f;
#else

#endif
}

void m4::lookAtUpright(const v3& _Pos,const v3& _Tgt)
{
	v3 z = (_Tgt-_Pos).unit();
	const float s = 1.f/sqrtf(z[0]*z[0]+z[2]*z[2]);
	v3 x( z[2]*s, 0, -z[0]*s );
	v3 y( x[0]*z[1], x[0]*z[2]-x[2]*z[0], -x[0]*z[1] );

	f[ 0]=x.x(); f[ 1]=x.y(); f[ 2]=x.z(); f[ 3]=0.f;
	f[ 4]=y.x(); f[ 5]=y.y(); f[ 6]=y.z(); f[ 7]=0.f;
	f[ 8]=z.x(); f[ 9]=z.y(); f[10]=z.z(); f[11]=0.f;
	f[12]=_Pos.x();  f[13]=_Pos.y();  f[14]=_Pos.z();  f[15]=1.f;
}

void m4::reorthonormalise()
{
/*
	v3 z = (v3(f[8],f[9],f[10])).unit();
	v3 y = v3(f[4],f[5],f[6]);
	v3 x = v3::cross(y,z).unit();
	y = v3::cross(z,x);

	f[ 0]=x.x(); f[ 1]=x.y(); f[ 2]=x.z();
	f[ 4]=y.x(); f[ 5]=y.y(); f[ 6]=y.z();
	f[ 8]=z.x(); f[ 9]=z.y(); f[10]=z.z();
/*/
	v3 x = (v3(f[ 0],f[ 1],f[ 2])).unit();
	v3 y = (v3(f[ 4],f[ 5],f[ 6])).unit();
	v3 z = (v3(f[ 8],f[ 9],f[10])).unit();

	f[ 0]=x.x(); f[ 1]=x.y(); f[ 2]=x.z();
	f[ 4]=y.x(); f[ 5]=y.y(); f[ 6]=y.z();
	f[ 8]=z.x(); f[ 9]=z.y(); f[10]=z.z();
//*/
}


void m4::print(const std::string _Str) const
{
	if( _Str!="" ) printf( "%s\n", _Str.c_str() );
	printf( "%.3f %.3f %.3f %.3f\n", 	f[0], f[4], f[ 8], f[12] );
	printf( "%.3f %.3f %.3f %.3f\n", 	f[1], f[5], f[ 9], f[13] );
	printf( "%.3f %.3f %.3f %.3f\n", 	f[2], f[6], f[10], f[14] );
	printf( "%.3f %.3f %.3f %.3f\n\n", 	f[3], f[7], f[11], f[15] );
}

/*
Ray m4::createRayFromVector( const v3 _Value ) const
{
	v3 p = v3(f[12],f[13],f[14]);

	v3 pos = v3(
		-v3::dot(v3(f[ 0],f[ 4],f[ 8]),p),
		-v3::dot(v3(f[ 1],f[ 5],f[ 9]),p),
		-v3::dot(v3(f[ 2],f[ 6],f[10]),p) );

	v3 dir = v3(
		v3::dot(v3(f[ 0],f[ 4],f[ 8]),_Value),
		v3::dot(v3(f[ 1],f[ 5],f[ 9]),_Value),
		v3::dot(v3(f[ 2],f[ 6],f[10]),_Value) );

	return Ray( pos, dir );
}
*/

} // namespace dalelib
